#ifndef I_LIB_FEATURES_I_BASE_H
#define I_LIB_FEATURES_I_BASE_H

#include <cassert>
#include <vector>
#include "../core/i_blas.h"
#include "../core/i_struct.h"
#include "../algorithm/i_sort.h"

namespace idl
{
    const int I_DEFAULT_FEATURE_SAMPLE = 1;
    const float I_DEFAULT_FEATURE_HESSIAN_RESPONSE_THRESHOLD = 0.0004f;
    const int I_DEFAULT_NR_INTEREST_POINTS_PER_IMAGE = 3000;
    const int I_DEFAULT_NR_BLOCKS_HORIZONTAL = 10;
    const int I_DEFAULT_NR_BLOCKS_VERTICAL = 10;

    /*the patch size is nxn*/
    template<typename DATATYPE, typename COORDTYPE, unsigned int n/*patch width*/>
    class SquaredPatchInterestPointBase
    {
    public:
        typedef DATATYPE  DATA_TYPE;
        typedef COORDTYPE COORD_TYPE;
        static const unsigned int WIDTH = n;
        static const unsigned int DIM = n*n;
        SquaredPatchInterestPointBase()
            :x(0), y(0), scale(0), orientation(0) {
            i_zero<DATATYPE>(descriptor, DIM);
        }
        SquaredPatchInterestPointBase(COORDTYPE col, COORDTYPE row)
            :x(col), y(row), scale(0), orientation(0) {
            i_zero<DATATYPE>(descriptor, DIM);
        }
        SquaredPatchInterestPointBase(const SquaredPatchInterestPointBase<DATATYPE,
                                      COORDTYPE, n>& ipt) {
            x = ipt.x;
            y = ipt.y;
            scale = ipt.scale;
            orientation = ipt.orientation;
            i_copy<DATATYPE>(ipt.descriptor, descriptor, DIM);
        }
        SquaredPatchInterestPointBase& operator=(const SquaredPatchInterestPointBase<DATATYPE,
                                                 COORDTYPE, n>& ipt) {
            this->x = ipt.x;
            this->y = ipt.y;
            this->scale = ipt.scale;
            this->orientation = ipt.orientation;
            i_copy<DATATYPE>(ipt.descriptor, this->descriptor, DIM);
            return(*this);
        }
        virtual ~SquaredPatchInterestPointBase() {
        }
        static int width() {
            return (int)n;
        }
        static int dim() {
            return (int)n*n;
        }
        COORDTYPE x, y;
        char scale, orientation;
        DATATYPE descriptor[DIM];
    };
    
    template<typename T, unsigned int n>
    class InterestPointBase
    {
    public:
        typedef T DATATYPE;
        static const int DIM = n;
        InterestPointBase() :x(0.f), y(0.f), scale(0.f), orientation(0.f), response(0.f) {
            i_zero<T>(descriptor, DIM);
        }
        InterestPointBase(const InterestPointBase<T, n>& ipt) {
            type = ipt.type;
            x = ipt.x;
            y = ipt.y;
            scale = ipt.scale;
            orientation = ipt.orientation;
            response = ipt.response;
            i_copy<T>(ipt.descriptor, descriptor, DIM);
        }
        InterestPointBase& operator=(const InterestPointBase<T, n>& ipt) {
            this->type = ipt.type;
            this->x = ipt.x;
            this->y = ipt.y;
            this->scale = ipt.scale;
            this->orientation = ipt.orientation;
            this->response = ipt.response;
            i_copy<T>(ipt.descriptor, this->descriptor, DIM);
            return(*this);
        }
        virtual ~InterestPointBase() {}
        static int dim() {
            return (int)n;
        }
        int type;
        float x, y;
        float scale;
        float orientation;
        float response;
        T descriptor[DIM];
    };
    
    template<typename T>
    class InterestPointResponseLayer
    {
    public:
        typedef       T&       reference;
        typedef const T& const_reference;
    
        InterestPointResponseLayer() :
            width(0), height(0), size(0), initialized(false), responses(NULL) {
        };
        
        InterestPointResponseLayer(int w, int h) : 
            initialized(false), responses(NULL) {
            assert(width > 0 && height > 0);
            this->width = width;
            this->height = height;
            this->size = width*height;
            responses = i_alloc2<T>(height, width);
            if (responses) {
                initialized = true;
            }
        };
    
        InterestPointResponseLayer(int width, int height, T init_val) : 
            initialized(false), responses(NULL) {
            assert(width > 0 && height > 0);
            this->width = width;
            this->height = height;
            this->size = width*height;
            responses = i_alloc2<T>(height, width);
            if (responses) {
                initialized = true;
                i_fill(responses[0], size, init_val);
            }
        };
    
        virtual ~InterestPointResponseLayer() { 
            i_free2<T>(responses); 
        };
    
        T get_response(int x, int y) { 
            assert(y < height && y >= 0 && x < width && x >= 0); 
            return responses[y][x];
        }
        T* get_response(int row) { 
            assert(row < height && row >= 0); 
            return responses[row]; 
        }
        T* get_response(){ 
            return responses[0]; 
        }
        const T* get_const_response(int row) const { 
            assert(row < height && row >= 0); 
            return responses[row]; 
        }
        const T* get_const_response() const { 
            return responses[0]; 
        }
        T* operator[](int row) { 
            assert(row < height && row >= 0); 
            return responses[row]; 
        }
        const T* operator[](int row) const { 
            assert(row < height && row >= 0); 
            return responses[row]; 
        }
        reference operator()(int row, int col) { 
            assert(row < height && row >= 0 && col < width && col >= 0); 
            return responses[row][col]; 
        }
        const_reference operator()(int row, int col) const { 
            assert(row < height && row >= 0 && col < width && col >= 0); 
            return responses[row][col]; 
        }
        int width, height, size;
        bool initialized;
    protected:
        T **responses;
    };

    template<typename InterestPointType, typename DescriptorType>
    inline int i_match_interestpoints(const std::vector<InterestPointType> &ipts_1, 
        const std::vector<InterestPointType> &ipts_2, 
        std::vector<Pair<InterestPointType, InterestPointType> >& correspondences, 
        double ratio_thre = 0.6)
    {
        correspondences.clear();
        correspondences.reserve(ipts_1.size());
        if (ipts_1.empty() || ipts_2.empty())
        {
            return 0;
        }
        int i, j, bestpos, type;
        DescriptorType best, secondbest, dist;
    
        const int n = ipts_1[0].dim();
        assert(n == ipts_2[0].dim());
    
        for (i = 0; i < (int)ipts_1.size(); i++)
        {
            best = secondbest = Constant<DescriptorType>::MAX_VAL();
            bestpos = -1;
            type = ipts_1[i].type;
            for (j = 0; j < (int)ipts_2.size(); j++)
            {
                if (type != ipts_2[j].type)
                {
                    continue;
                }
                dist = i_sqrt(i_squaresum_diff<DescriptorType>(
                    ipts_1[i].descriptor, 
                    ipts_2[j].descriptor, 
                    n));
                
                if (dist < best) {// if this feature matches better than current best
                    bestpos = j;
                    secondbest = best;
                    best = dist;
                } else {
                    if (dist < secondbest) {// this feature matches better than second best
                        secondbest = dist;
                    }
                }
            }
            // If match has a best:secondbest ratio < ratio_thre then consider as a match
            if (bestpos >= 0 && i_div((double)best, (double)secondbest) < ratio_thre)
            {
                // push the match to the list
                correspondences.push_back(i_make_pair(ipts_1[i], ipts_2[bestpos]));
            }
        }
        return (int)correspondences.size();
    }

    template<typename InterestPointType, typename DescriptorType>
    inline int i_match_interestpoints(const std::vector<InterestPointType> &ipts_1, 
        const std::vector<InterestPointType> &ipts_2, 
        std::vector<Pair<int, int> >& correspondences, 
        double ratio_thre = 0.6)
    {
        correspondences.clear();
        correspondences.reserve(ipts_1.size());
        if (ipts_1.empty() || ipts_2.empty())
        {
            return 0;
        }
        int i, j, bestpos, type;
        DescriptorType best, secondbest, dist;
    
        const int n = ipts_1[0].dim();
        assert(n == ipts_2[0].dim());
    
        for (i = 0; i < (int)ipts_1.size(); i++)
        {
            best = secondbest = Constant<DescriptorType>::MAX_VAL();
            bestpos = -1;
            type = ipts_1[i].type;
            for (j = 0; j < (int)ipts_2.size(); j++)
            {
                if (type != ipts_2[j].type) {
                    continue;
                }
                dist = i_sqrt(i_squaresum_diff<DescriptorType>(
                    ipts_1[i].descriptor, 
                    ipts_2[j].descriptor, 
                    n));
    
                if (dist < best) {// if this feature matches better than current best
                    bestpos = j;
                    secondbest = best;
                    best = dist;
                } else {
                    if (dist < secondbest) {// this feature matches better than second best
                        secondbest = dist;
                    }
                }
            }
            // If match has a best:secondbest ratio < ratio_thre then consider as a match
            if (bestpos >= 0 && i_div((double)best, (double)secondbest) < ratio_thre) {
                // push the match to the list
                correspondences.push_back(i_make_pair(i, bestpos));
            }
        }
        return (int)correspondences.size();
    }
} /*namespace idl*/

#endif
